//
// Created by zengyuhao on 25-4-30.
//

#ifndef ZORAL_TRAJECTORY_H
#define ZORAL_TRAJECTORY_H

#include <vector>
#include "plan/plan_data_structure.hpp"
namespace ZORAL
{
    class Trajectory
    {
    private:
        std::vector<std::vector<double>> traj_pos_;
        std::vector<std::vector<double>> traj_vel_;
        std::vector<std::vector<double>> traj_acc_;
        std::vector<double> max_vel_;
        std::vector<double> max_acc_;
        std::vector<double> max_jerk_;
        double min_move_time_;
        double sample_rate_;
        std::vector<double> traj_time_;
        double duration_time_;

    public:
        Trajectory() = default;

        inline void clear()
        {
            traj_pos_.clear();
            traj_vel_.clear();
            traj_acc_.clear();
        }

        inline void pushbackPos(const std::vector<double> &q)
        {
            traj_pos_.push_back(q);
        }

        inline void pushbackVel(const std::vector<double> &qd)
        {
            traj_vel_.push_back(qd);
        }

        inline void pushbackAcc(const std::vector<double> &qdd)
        {
            traj_acc_.push_back(qdd);
        }

        inline std::vector<std::vector<double>> getPos()
        {
            return traj_pos_;
        }
        inline std::vector<std::vector<double>> getVel()
        {
            return traj_vel_;
        }
        inline std::vector<std::vector<double>> getAcc()
        {
            return traj_acc_;
        }
        inline double getDuration()
        {
            return duration_time_;
        }

        inline void setMaxVel(const std::vector<double> max_vel)
        {
            max_vel_ = max_vel;
        }
        inline void setMaxAcc(const std::vector<double> max_acc)
        {
            max_acc_ = max_acc;
        }
        inline void setMaxJerk(const std::vector<double> max_jerk)
        {
            max_jerk_ = max_jerk;
        }
        inline void setMinMoveTime(const double &min_move_time)
        {
            min_move_time_ = min_move_time;
        }
        inline void setSampleRate(const double &sample_rate)
        {
            sample_rate_ = sample_rate;
        }
        int compute(const TrajSolveType &plan_algo, const std::vector<double> &pos_start, const std::vector<double> &pos_end,
                    const std::vector<double> &vel_start, const std::vector<double> &vel_end,
                    const std::vector<double> &acc_start, const std::vector<double> &acc_end);
    };
} // namespace ZORAL
#endif // ZORAL_TRAJECTORY_H
